package oneD.dynamics;

import oneD.fem.structure.Structure;
import inf.jlinalg.ArrayVector;
import inf.jlinalg.IMatrix;
import inf.jlinalg.IVector;
import inf.jlinalg.Vector3D;

public class DynamicStructure {
	private Structure struct_;
	private DynamicNode[] nodes_;
	private int DisCounter_;
	private int VelCounter_;
	private int AccCounter_;
	private double timeStepForPlot_;

	public DynamicStructure(Structure struct) {
		this.struct_ = struct;
		this.DisCounter_ = 0;
		this.VelCounter_ = 0;
		this.AccCounter_ = 0;
	}

	public void Initialize() {
		this.nodes_ = new DynamicNode[this.struct_.getNumberOfNodes()];
		for (int i = 0; i < this.nodes_.length; i++)
			nodes_[i] = new DynamicNode();
	}

	public void setTimeStepForPlot(double timeStep) {
		this.timeStepForPlot_ = timeStep;
	}

	public double getTimeStepForPlot() {
		return this.timeStepForPlot_;
	}

	public int getNumberOfStep() {
		return this.DisCounter_;
	}

	public DynamicNode getNode(int idx) {
		return this.nodes_[idx];
	}

	public Structure getStaticStructure() {
		return this.struct_;
	}

	public IVector getStaticDisplacementVector() {
		/*
		 * this return the displacement vector of the static structure after
		 * applying Dirichlet boundary condition
		 */
		return getDynamicNodeDataInVectorForm(0, 0);
	}

	public IVector getDisplacementVector(int step) {
		/*
		 * this return the displacement vector of the structure after applying
		 * Dirichlet boundary condition
		 */
		return getDynamicNodeDataInVectorForm(step, 1);
	}

	public IVector getVelocityVector(int step) {
		/*
		 * this return the velocity vector of the structure after applying
		 * Dirichlet boundary condition
		 */
		return getDynamicNodeDataInVectorForm(step, 2);
	}

	public IVector getAccelerationVector(int step) {
		/*
		 * this return the acceleration vector of the structure after applying
		 * Dirichlet boundary condition
		 */
		return getDynamicNodeDataInVectorForm(step, 3);
	}

	private IVector getDynamicNodeDataInVectorForm(int step, int type) {
		/*
		 * type = 1: return dynamic displacement vector type = 2: return dynamic
		 * velocity vector type = 3: return dynamic acceleration vector type =
		 * 0/other: return static displacement vector
		 */
		int NEQ = this.struct_.getNumberOfEquations();
		IVector V = new ArrayVector(NEQ);
		for (int i = 0; i < this.struct_.getNumberOfNodes(); i++) {
			int[] id = this.struct_.getNode(i).getDOFNumbers();
			double[] v = type == 1 ? this.nodes_[i].getDisplacement(step)
					.toArray() : (type == 2 ? this.nodes_[i].getVelocity(step)
					.toArray() : (type == 3 ? this.nodes_[i].getAcceleration(
					step).toArray() : this.struct_.getNode(i).getDisplacement()
					.toArray()));
			if (id[0] != -1)
				V.set(id[0], v[0]);
			if (id[1] != -1)
				V.set(id[1], v[1]);
			if (id[2] != -1)
				V.set(id[2], v[2]);
		}
		return V;
	}

	public void addDisplacement(IVector U) {
		addDynamicNodeData(U, 1);
		DisCounter_++;
	}

	public void addVelocity(IVector U) {
		addDynamicNodeData(U, 2);
		VelCounter_++;
	}

	public void addAcceleration(IVector U) {
		addDynamicNodeData(U, 3);
		AccCounter_++;
	}

	private void addDynamicNodeData(IVector U, int type) {
		for (int i = 0; i < this.nodes_.length; i++) {
			double[] u = new double[3];
			int[] dofNumbers = this.struct_.getNode(i).getDOFNumbers();
			for (int j = 0; j < 3; j++)
				if (dofNumbers[j] == -1)
					u[j] = 0;
				else
					u[j] = U.get(dofNumbers[j]);
			if (type == 1)
				this.nodes_[i].addDisplacement(new Vector3D(u));
			else if (type == 2)
				this.nodes_[i].addVelocity(new Vector3D(u));
			else if (type == 3)
				this.nodes_[i].addAcceleration(new Vector3D(u));
			else {
			}
		}
	}

	public void assembleKeffForCDM(IMatrix Keff, double alpha1, double alpha2,
			double dt) {
		for (int i = 0; i < this.struct_.getNumberOfElements(); i++) {
			IMatrix ke = this.struct_.getElement(i).computeStiffnessMatrix();
			IMatrix me = this.struct_.getElement(i).computeMassMatrix();
			int[] dofNumbers = this.struct_.getElement(i).getDOFNumbers();
			for (int p = 0; p < 6; p++)
				for (int q = 0; q < 6; q++)
					if (dofNumbers[p] != -1 && dofNumbers[q] != -1)
						Keff.add(dofNumbers[p], dofNumbers[q],
								(1 / dt / dt + alpha1 / 2 / dt) * me.get(p, q)
										+ alpha2 / 2 / dt * ke.get(p, q));
		}
	}

	public void assembleKdg1(IMatrix Kdg, IMatrix K, IMatrix M, double alpha1,
			double alpha2, double dt) {
		/*
		 * assemble efficient stiffness matrix for discontinuous Galerkin method
		 * p=pbar=1 assumming An=Au=K,Av=M
		 */
		int m = Kdg.getRowCount() / 4;
		for (int i = 0; i < this.struct_.getNumberOfElements(); i++) {
			IMatrix ke = this.struct_.getElement(i).computeStiffnessMatrix();
			IMatrix me = this.struct_.getElement(i).computeMassMatrix();
			IMatrix An = ke;
			IMatrix Au = ke;
			IMatrix Av = me;

			int[] dofNumbers = this.struct_.getElement(i).getDOFNumbers();
			for (int p = 0; p < 6; p++)
				for (int q = 0; q < 6; q++)
					if (dofNumbers[p] != -1 && dofNumbers[q] != -1) {
						K.add(dofNumbers[p], dofNumbers[q], ke.get(p, q));
						M.add(dofNumbers[p], dofNumbers[q], me.get(p, q));

						// facilitate (1,1) -An/2+Au
						Kdg.add(dofNumbers[p], dofNumbers[q],
								-0.5 * An.get(p, q) + Au.get(p, q));
						// facilitate (1,2)
						Kdg.add(dofNumbers[p], dofNumbers[q] + m,
								0.5 * An.get(p, q));
						// facilitate (1,3)
						Kdg.add(dofNumbers[p], dofNumbers[q] + 2 * m, -(dt / 3)
								* An.get(p, q));
						// facilitate (1,4)
						Kdg.add(dofNumbers[p], dofNumbers[q] + 3 * m, -(dt / 6)
								* An.get(p, q));
						// facilitate (2,1)
						Kdg.add(dofNumbers[p] + m, dofNumbers[q],
								-0.5 * An.get(p, q));
						// facilitate (2,2)
						Kdg.add(dofNumbers[p] + m, dofNumbers[q] + m,
								0.5 * An.get(p, q));
						// facilitate (2,3)
						Kdg.add(dofNumbers[p] + m, dofNumbers[q] + 2 * m,
								-(dt / 6) * An.get(p, q));
						// facilitate (2,4)
						Kdg.add(dofNumbers[p] + m, dofNumbers[q] + 3 * m,
								-(dt / 3) * An.get(p, q));
						// facilitate (3,1)
						Kdg.add(dofNumbers[p] + 2 * m, dofNumbers[q], (dt / 3)
								* ke.get(p, q));
						// facilitate (3,2)
						Kdg.add(dofNumbers[p] + 2 * m, dofNumbers[q] + m,
								(dt / 6) * ke.get(p, q));
						// facilitate (3,3)
						Kdg.add(dofNumbers[p] + 2 * m,
								dofNumbers[q] + 2 * m,
								-0.5
										* me.get(p, q)
										+ (dt / 3)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)) + Av.get(p, q));
						// facilitate (3,4)
						Kdg.add(dofNumbers[p] + 2 * m,
								dofNumbers[q] + 3 * m,
								0.5
										* me.get(p, q)
										+ (dt / 6)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)));
						// facilitate (4,1)
						Kdg.add(dofNumbers[p] + 3 * m, dofNumbers[q], (dt / 6)
								* ke.get(p, q));

						// facilitate (4,2)
						Kdg.add(dofNumbers[p] + 3 * m, dofNumbers[q] + m,
								(dt / 3) * ke.get(p, q));

						// facilitate (4,3)
						Kdg.add(dofNumbers[p] + 3 * m,
								dofNumbers[q] + 2 * m,
								-0.5
										* me.get(p, q)
										+ (dt / 6)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)));

						// facilitate (4,4)
						Kdg.add(dofNumbers[p] + 3 * m,
								dofNumbers[q] + 3 * m,
								0.5
										* me.get(p, q)
										+ (dt / 3)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)));
					}
		}

	}

	public void assembleKdg2(IMatrix Kdg, IMatrix K, IMatrix M, double alpha1,
			double alpha2, double dt) {
		/*
		 * assemble efficient stiffness matrix for discontinuous Galerkin method
		 * p=pbar=2 assumming An=Au=K,Av=M
		 */
		int m = Kdg.getRowCount() / 6;
		for (int i = 0; i < this.struct_.getNumberOfElements(); i++) {
			IMatrix ke = this.struct_.getElement(i).computeStiffnessMatrix();
			IMatrix me = this.struct_.getElement(i).computeMassMatrix();
			IMatrix An = ke;
			IMatrix Au = ke;
			IMatrix Av = me;
			double Jt = dt / 2;

			int[] dofNumbers = this.struct_.getElement(i).getDOFNumbers();
			for (int p = 0; p < 6; p++)
				for (int q = 0; q < 6; q++)
					if (dofNumbers[p] != -1 && dofNumbers[q] != -1) {
						K.add(dofNumbers[p], dofNumbers[q], ke.get(p, q));
						M.add(dofNumbers[p], dofNumbers[q], me.get(p, q));

						// facilitate (1,1)
						Kdg.add(dofNumbers[p], dofNumbers[q],
								-0.5 * An.get(p, q) + Au.get(p, q));
						// facilitate (1,2)
						Kdg.add(dofNumbers[p], dofNumbers[q] + m, (2.0 / 3)
								* An.get(p, q));
						// facilitate (1,3)
						Kdg.add(dofNumbers[p], dofNumbers[q] + 2 * m,
								-(1.0 / 6) * An.get(p, q));
						// facilitate (1,4)
						Kdg.add(dofNumbers[p], dofNumbers[q] + 3 * m,
								-(4.0 / 15) * An.get(p, q) * Jt);
						// facilitate (1,5)
						Kdg.add(dofNumbers[p], dofNumbers[q] + 4 * m,
								-(2.0 / 15) * An.get(p, q) * Jt);
						// facilitate (1,6)
						Kdg.add(dofNumbers[p], dofNumbers[q] + 5 * m,
								(1.0 / 15) * An.get(p, q) * Jt);
						// facilitate (2,1)
						Kdg.add(dofNumbers[p] + m, dofNumbers[q], -(2.0 / 3)
								* An.get(p, q));
						// facilitate (2,2)
						// facilitate (2,3)
						Kdg.add(dofNumbers[p] + m, dofNumbers[q] + 2 * m,
								(2.0 / 3) * An.get(p, q));
						// facilitate (2,4)
						Kdg.add(dofNumbers[p] + m, dofNumbers[q] + 3 * m,
								-(2.0 / 15) * An.get(p, q) * Jt);
						// facilitate (2,5)
						Kdg.add(dofNumbers[p] + m, dofNumbers[q] + 4 * m,
								-(16.0 / 15) * An.get(p, q) * Jt);
						// facilitate (2,6)
						Kdg.add(dofNumbers[p] + m, dofNumbers[q] + 5 * m,
								-(2.0 / 15) * An.get(p, q) * Jt);
						// facilitate (3,1)
						Kdg.add(dofNumbers[p] + 2 * m, dofNumbers[q], (1.0 / 6)
								* An.get(p, q));
						// facilitate (3,2)
						Kdg.add(dofNumbers[p] + 2 * m, dofNumbers[q] + m,
								-(2.0 / 3) * An.get(p, q));
						// facilitate (3,3)
						Kdg.add(dofNumbers[p] + 2 * m, dofNumbers[q] + 2 * m,
								0.5 * An.get(p, q));
						// facilitate (3,4)
						Kdg.add(dofNumbers[p] + 2 * m, dofNumbers[q] + 3 * m,
								(1.0 / 15) * An.get(p, q) * Jt);
						// facilitate (3,5)
						Kdg.add(dofNumbers[p] + 2 * m, dofNumbers[q] + 4 * m,
								-(2.0 / 15) * An.get(p, q) * Jt);
						// facilitate (3,6)
						Kdg.add(dofNumbers[p] + 2 * m, dofNumbers[q] + 5 * m,
								-(4.0 / 15) * An.get(p, q) * Jt);
						// facilitate (4,1)
						Kdg.add(dofNumbers[p] + 3 * m, dofNumbers[q],
								(4.0 / 15) * ke.get(p, q) * Jt);
						// facilitate (4,2)
						Kdg.add(dofNumbers[p] + 3 * m, dofNumbers[q] + m,
								(2.0 / 15) * ke.get(p, q) * Jt);
						// facilitate (4,3)
						Kdg.add(dofNumbers[p] + 3 * m, dofNumbers[q] + 2 * m,
								-(1.0 / 15) * ke.get(p, q) * Jt);
						// facilitate (4,4)
						Kdg.add(dofNumbers[p] + 3 * m,
								dofNumbers[q] + 3 * m,
								-0.5
										* me.get(p, q)
										+ (4.0 / 15)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)) * Jt
										+ Av.get(p, q));
						// facilitate (4,5)
						Kdg.add(dofNumbers[p] + 3 * m,
								dofNumbers[q] + 4 * m,
								(2.0 / 3)
										* me.get(p, q)
										+ (2.0 / 15)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)) * Jt);
						// facilitate (4,6)
						Kdg.add(dofNumbers[p] + 3 * m,
								dofNumbers[q] + 5 * m,
								-(1.0 / 6)
										* me.get(p, q)
										- (1.0 / 15)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)) * Jt);
						// facilitate (5,1)
						Kdg.add(dofNumbers[p] + 4 * m, dofNumbers[q],
								(2.0 / 15) * ke.get(p, q) * Jt);
						// facilitate (5,2)
						Kdg.add(dofNumbers[p] + 4 * m, dofNumbers[q] + m,
								(16.0 / 15) * ke.get(p, q) * Jt);
						// facilitate (5,3)
						Kdg.add(dofNumbers[p] + 4 * m, dofNumbers[q] + 2 * m,
								(2.0 / 15) * ke.get(p, q) * Jt);
						// facilitate (5,4)
						Kdg.add(dofNumbers[p] + 4 * m,
								dofNumbers[q] + 3 * m,
								-(2.0 / 3)
										* me.get(p, q)
										+ (2.0 / 15)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)) * Jt);
						// facilitate (5,5)
						Kdg.add(dofNumbers[p] + 4 * m,
								dofNumbers[q] + 4 * m,
								(16.0 / 15)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)) * Jt);
						// facilitate (5,6)
						Kdg.add(dofNumbers[p] + 4 * m,
								dofNumbers[q] + 5 * m,
								(2.0 / 3)
										* me.get(p, q)
										+ (2.0 / 15)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)) * Jt);
						// facilitate (6,1)
						Kdg.add(dofNumbers[p] + 5 * m, dofNumbers[q],
								-(1.0 / 15) * ke.get(p, q) * Jt);
						// facilitate (6,2)
						Kdg.add(dofNumbers[p] + 5 * m, dofNumbers[q] + m,
								(2.0 / 15) * ke.get(p, q) * Jt);
						// facilitate (6,3)
						Kdg.add(dofNumbers[p] + 5 * m, dofNumbers[q] + 2 * m,
								(4.0 / 15) * ke.get(p, q) * Jt);
						// facilitate (6,4)
						Kdg.add(dofNumbers[p] + 5 * m,
								dofNumbers[q] + 3 * m,
								(1.0 / 6)
										* me.get(p, q)
										- (1.0 / 15)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)) * Jt);
						// facilitate (6,5)
						Kdg.add(dofNumbers[p] + 5 * m,
								dofNumbers[q] + 4 * m,
								-(2.0 / 3)
										* me.get(p, q)
										+ (2.0 / 15)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)) * Jt);
						// facilitate (6,6)
						Kdg.add(dofNumbers[p] + 5 * m,
								dofNumbers[q] + 5 * m,
								0.5
										* me.get(p, q)
										+ (4.0 / 15)
										* (alpha1 * me.get(p, q) + alpha2
												* ke.get(p, q)) * Jt);

					}
		}
	}

	public void assembleKdgp(int pDim, IMatrix Kdg, IMatrix K, IMatrix M,
			double alpha1, double alpha2, double dt) {
		/*
		 * assemble efficient stiffness matrix for discontinuous Galerkin method
		 * p=pbar assumming An=Au=K,Av=M
		 */
		int m = Kdg.getRowCount() / (2 * (pDim + 1));
		for (int k = 0; k < this.struct_.getNumberOfElements(); k++) {
			IMatrix ke = this.struct_.getElement(k).computeStiffnessMatrix();
			IMatrix me = this.struct_.getElement(k).computeMassMatrix();
			IMatrix An = ke;
			IMatrix Au = ke;
			IMatrix Av = me;
			dGDataset dG = new dGDataset(pDim);
			double Jt = dt / 2;

			int[] dofNumbers = this.struct_.getElement(k).getDOFNumbers();
			for (int p = 0; p < 6; p++)
				for (int q = 0; q < 6; q++)
					if (dofNumbers[p] != -1 && dofNumbers[q] != -1) {
						K.add(dofNumbers[p], dofNumbers[q], ke.get(p, q));
						M.add(dofNumbers[p], dofNumbers[q], me.get(p, q));

						for (int i = 0; i < pDim + 1; i++)
							for (int j = 0; j < pDim + 1; j++) {
								Kdg.add(dofNumbers[p] + i * m, dofNumbers[q]
										+ j * m, dG.getIt(i, j) * An.get(p, q));
								Kdg.add(dofNumbers[p] + i * m, dofNumbers[q]
										+ j * m + (pDim + 1) * m,
										-dG.getI(i, j) * An.get(p, q) * Jt);
								Kdg.add(dofNumbers[p] + i * m + (pDim + 1) * m,
										dofNumbers[q] + j * m, dG.getI(i, j)
												* ke.get(p, q) * Jt);
								Kdg.add(dofNumbers[p] + i * m + (pDim + 1) * m,
										dofNumbers[q] + j * m + (pDim + 1) * m,
										dG.getIt(i, j)
												* me.get(p, q)
												+ dG.getI(i, j)
												* (alpha1 * me.get(p, q) + alpha2
														* ke.get(p, q)) * Jt);

							}
						Kdg.add(dofNumbers[p], dofNumbers[q], Au.get(p, q));
						Kdg.add(dofNumbers[p] + (pDim + 1) * m, dofNumbers[q]
								+ (pDim + 1) * m, Av.get(p, q));
					}
		}
	}

	public void print() {
		System.out.println("Dynamic response of structure");
		System.out.print("Step\t");
		for (int i = 0; i < this.nodes_.length; i++)
			System.out.print("N" + i + ",u\t\t\t\t\t\t" + "N" + i
					+ ",ud\t\t\t\t\t\t" + "N" + i + ",udd\t\t\t\t\t\t");
		System.out.println();
		for (int i = 0; i < this.DisCounter_; i++) {
			System.out.print(i + "\t");
			for (int j = 0; j < this.nodes_.length; j++)
				System.out.print("{" + this.nodes_[j].getDisplacement(i)
						+ "}\t{" + this.nodes_[j].getVelocity(i) + "}\t{"
						+ this.nodes_[j].getAcceleration(i) + "}");
			System.out.println();
		}
	}

	public FunctionPlotter1D plot1(int nodeId, String axis, String type) {
		/*
		 * function to plot response w.r.t time set type = "" to get plot for
		 * displacement set type = "d" to get plot for velocity set type = "dd"
		 * to get plot for acceleration
		 */
		FunctionPlotter1D fp = new FunctionPlotter1D();
		String plotName = "u" + nodeId + axis + type;
		fp.registerPlotName(plotName);
		int axisId = axis == "x" ? 0 : axis == "y" ? 1 : 2;
		int plotType = type == "" ? 0 : type == "d" ? 1 : 2;
		for (int i = 0; i < this.DisCounter_; i++) {
			fp.insertData(i * this.timeStepForPlot_,
					this.nodes_[nodeId].get(i, plotType).get(axisId), plotName);
		}
		return fp;
	}

	private int getAxisNumber(String axisName) {
		return axisName == "x" ? 0 : axisName == "y" ? 1 : 2;
	}

	private int getTypeNumber(String typeName) {
		return typeName == "" ? 0 : typeName == "d" ? 1 : 2;
	}

	public FunctionPlotter1D plot1combination(int[] nodeIdSet,
			String[] axisSet, String[] typeSet) {
		/*
		 * function to plot response w.r.t time set type = "" to get plot for
		 * displacement set type = "d" to get plot for velocity set type = "dd"
		 * to get plot for acceleration
		 */
		FunctionPlotter1D fp = new FunctionPlotter1D();
		for (int i = 0; i < nodeIdSet.length; i++)
			for (int j = 0; j < axisSet.length; j++)
				for (int k = 0; k < typeSet.length; k++) {
					String plotName = "u" + nodeIdSet[i] + axisSet[j]
							+ typeSet[k];
					fp.registerPlotName(plotName);
					for (int l = 0; l < this.DisCounter_; l++) {
						fp.insertData(
								l * this.timeStepForPlot_,
								this.nodes_[nodeIdSet[i]].get(l,
										this.getTypeNumber(typeSet[k])).get(
										this.getAxisNumber(axisSet[j])),
								plotName);
					}
				}
		return fp;
	}

	public FunctionPlotter1D plot2(int nodeId, String axis) {
		/*
		 * function to plot displacement and velocity relation in the same axis
		 */
		FunctionPlotter1D fp = new FunctionPlotter1D();
		String plotName = "u" + axis + nodeId + "-" + "u" + nodeId + axis + "d";
		fp.registerPlotName(plotName);
		int axisId = axis == "x" ? 0 : axis == "y" ? 1 : 2;
		for (int i = 0; i < this.DisCounter_; i++) {
			fp.insertData(this.nodes_[nodeId].getDisplacement(i).get(axisId),
					this.nodes_[nodeId].getVelocity(i).get(axisId), plotName);
		}
		return fp;
	}

	public Plot1D plot() {
//		Plot1D p = new Plot1D();
//		p.setDt(this.timeStepForPlot_);
//		p.setN(this.DisCounter_);
//		p.initialize();
//		for (int i = 0; i < this.struct_.getNumberOfNodes(); i++)
//			for (int j = 0; j < 3; j++)
//				for (int k = 0; k < 3; k++) {
//					String plotName = "u" + i + this.getAxisName(j)
//							+ this.getTypeName(k);
//					p.registerPlotName(plotName);
//					for (int l = 0; l < this.DisCounter_; l++) {
//						p.insertData(l * this.timeStepForPlot_, this.nodes_[i]
//								.get(l, k).get(j), plotName);
//					}
//				}
//		return p;
		
		Plot1D p = new Plot1D(this);
		p.setDt(this.timeStepForPlot_);
		p.setN(this.DisCounter_);
		return p;
	}
}
